#include "MathModel.h"

GravityModel::GravityModel()
: wholeMass ( 0.0 )
{}

void GravityModel::AddBody ( Point2D point, double radius, double mass, Point2D vel )
{
    bodyInfoVect.push_back ( BodyInfo ( point, mass, radius, vel ));
    forceVect.push_back ( Point2D ( 0.0, 0.0 ));
    wholeMass += mass;
}

void GravityModel::SetTimeInterval ( double dt )
{
    this -> dt = dt;
}

void GravityModel::CalcStep()
{
    const double gravConst = 6.67428e-11;
    const double k1 = 2.0;
    const double k2 = -0.05;
    for ( int i = 0; i < bodyInfoVect.size(); ++i )
    {
        forceVect[i].x = forceVect[i].y = 0.0;
        for ( int j = 0; j < bodyInfoVect.size(); ++j )
        {
            if ( i == j )
                continue;
            double R = Distance ( bodyInfoVect[i].point, bodyInfoVect[j].point );
            double force = gravConst * bodyInfoVect[i].mass * bodyInfoVect[j].mass *
                ( k1 / pow ( R, 6 ) + k2 / pow ( R, 12 ));
            double ang = Angle ( bodyInfoVect[i].point, bodyInfoVect[j].point );
            forceVect[i].x += force * cos ( ang );
            forceVect[i].y += force * sin ( ang );
        }
    }
    for ( int i = 0; i < bodyInfoVect.size(); ++i )
    {
        bodyInfoVect[i].velocity.x += forceVect[i].x / bodyInfoVect[i].mass * dt;
        bodyInfoVect[i].velocity.y += forceVect[i].y / bodyInfoVect[i].mass * dt;
        bodyInfoVect[i].point.x += bodyInfoVect[i].velocity.x * dt;
        bodyInfoVect[i].point.y += bodyInfoVect[i].velocity.y * dt;
    }

    massCenter.x = massCenter.y = 0.0;
    for ( int i = 0; i < bodyInfoVect.size(); ++i )
    {
        massCenter.x += bodyInfoVect[i].point.x * bodyInfoVect[i].mass;
        massCenter.y += bodyInfoVect[i].point.y * bodyInfoVect[i].mass;
    }
    massCenter.x /= wholeMass;
    massCenter.y /= wholeMass;
}

int GravityModel::GetBodyCount()
{
    return bodyInfoVect.size();
}

void GravityModel::GetBodyPos ( int index, Point2D & point )
{
    ASSERT ( index >= 0 && index < bodyInfoVect.size());

    point = bodyInfoVect[index].point;
}

Point2D GravityModel::GetMassCenter()
{
    return massCenter;
}
